#include <animation/NodeAnimationTrack.h>

namespace ne
{
    NodeAnimationTrack::NodeAnimationTrack(Animation *parent, const uint16_t handle, Node *node)
        : AnimationTrack(parent, handle),
          mpTargetNode(node),
          mSplineBuildNeeded(false)
    {
    }

    TransformKeyFrame* NodeAnimationTrack::createTransformKeyFrame(const real time)
    {
        return dynamic_cast<TransformKeyFrame*>(createKeyFrame(time));
    }

    void NodeAnimationTrack::setAssociatedNode(Node *node)
    {
        mpTargetNode = node;
    }

    Node* NodeAnimationTrack::getAssociatedNode() const
    {
        return mpTargetNode;
    }

    TransformKeyFrame* NodeAnimationTrack::getTransformKeyFrame(const size_t index) const
    {
        return dynamic_cast<TransformKeyFrame*>(getKeyFrame(index));
    }

    void NodeAnimationTrack::getKeyFrameAtTime(const real time, KeyFrame &keyFrame) const
    {
        _sort();

        // Binary Search
        KeyFrame *baseBeginFrame = 0;
        KeyFrame *baseEndFrame = 0;

        if (getKeyFramesCount())
        {
            _binarySearchKeyFramesAtTime(0, getKeyFramesCount()-1, time, &baseBeginFrame, &baseEndFrame);
        }

        TransformKeyFrame *beginKeyFrame = dynamic_cast<TransformKeyFrame*>(baseBeginFrame);
        TransformKeyFrame *endKeyFrame = dynamic_cast<TransformKeyFrame*>(baseEndFrame);

        /* Normal Search
        TransformKeyFrame *beginKeyFrame = 0;
        TransformKeyFrame *endKeyFrame = 0;
        for (KeyFrameVectorIterator it=mKeyFrames.begin(); it!=mKeyFrames.end(); ++it)
        {
            real keyTime = (*it)->getTime();
            if (keyTime <= time)
            {
                beginKeyFrame = dynamic_cast<TransformKeyFrame*>(*it);
            }
            if (keyTime > time)
            {
                endKeyFrame = dynamic_cast<TransformKeyFrame*>(*it);
                break;
            }
        }
        */

        TransformKeyFrame *midKeyFrame = dynamic_cast<TransformKeyFrame*>(&keyFrame);
        if (beginKeyFrame == 0)
        {
            if (endKeyFrame == 0)
            {
                midKeyFrame->setTranslate(Vector3::ZERO);
                midKeyFrame->setRotate(Quaternion::IDENTITY);
                midKeyFrame->setScale(Vector3::ONE);
            }
            else
            {
                real u = time / endKeyFrame->getTime();
                Vector3 translate = endKeyFrame->getTranslate() * u;
                midKeyFrame->setTranslate(translate);

                Quaternion rotate = Quaternion::Slerp(Quaternion::IDENTITY, endKeyFrame->getRotate(), u, mUseShortestRotationPath);
                midKeyFrame->setRotate(rotate);

                Vector3 scale = Vector3::Lerp(Vector3::ONE, endKeyFrame->getScale(), u);
                midKeyFrame->setScale(scale);
            }
        }
        else
        {
            if (endKeyFrame == 0)
            {
                midKeyFrame->setTranslate(beginKeyFrame->getTranslate());
                midKeyFrame->setRotate(beginKeyFrame->getRotate());
                midKeyFrame->setScale(beginKeyFrame->getScale());
            }
            else
            {
                real u = (time - beginKeyFrame->getTime()) / (endKeyFrame->getTime() - beginKeyFrame->getTime());
                Vector3 translate = Vector3::Lerp(beginKeyFrame->getTranslate(), endKeyFrame->getTranslate(), u);
                midKeyFrame->setTranslate(translate);

                Quaternion rotate = Quaternion::Slerp(beginKeyFrame->getRotate(), endKeyFrame->getRotate(), u, mUseShortestRotationPath);
                midKeyFrame->setRotate(rotate);

                Vector3 scale = Vector3::Lerp(beginKeyFrame->getScale(), endKeyFrame->getScale(), u);
                midKeyFrame->setScale(scale);
            }
        }
    }

    void NodeAnimationTrack::getTransformKeyFrameAtTime(const real time, TransformKeyFrame &keyFrame) const
    {
        getKeyFrameAtTime(time, keyFrame);
    }

    void NodeAnimationTrack::setUseShortestRotationPath(const bool shortestPath)
    {
        mUseShortestRotationPath = shortestPath;
    }

    bool NodeAnimationTrack::getUseShortestRotationPath() const
    {
        return mUseShortestRotationPath;
    }

    void NodeAnimationTrack::apply(const real time, const real weight, const real scaleU)
    {
        applyToNode(mpTargetNode, time, weight, scaleU);
    }

    void NodeAnimationTrack::applyToNode(Node *node, const real time, const real weight, const real scaleU)
    {
        if (node == 0 || weight == 0.0f || mKeyFrames.empty())
        {
            return;
        }

        TransformKeyFrame currentKeyFrame(this, time);
        getTransformKeyFrameAtTime(time, currentKeyFrame);

        Vector3 translate = currentKeyFrame.getTranslate() * weight * scaleU;
        node->translate(translate);

        Quaternion rotate = Quaternion::Slerp(Quaternion::IDENTITY, currentKeyFrame.getRotate(), weight, mUseShortestRotationPath);
        node->rotate(rotate);

        Vector3 scale = currentKeyFrame.getScale();
        if (scaleU != 1.0f && scale != Vector3::ONE)
        {
            scale = Vector3::Lerp(Vector3::ONE, scale, scaleU);
        }
        node->scale(scale);
    }

    bool NodeAnimationTrack::hasNonZeroKeyFrames() const
    {
        for (KeyFrameVectorIterator it=mKeyFrames.begin(); it!=mKeyFrames.end(); ++it)
        {
            TransformKeyFrame *keyFrame = dynamic_cast<TransformKeyFrame*>(*it);
            Vector3 translate = keyFrame->getTranslate();
            Quaternion rotate = keyFrame->getRotate();
            Vector3 scale = keyFrame->getScale();
            Vector3 axis;
            real angle;
            rotate.toAxisAngle(axis, angle);

            if (!Math::Equal(translate.x, 0.0f) ||
                !Math::Equal(translate.y, 0.0f) ||
                !Math::Equal(translate.z, 0.0f) ||
                !Math::Equal(scale.x, 1.0f) ||
                !Math::Equal(scale.y, 1.0f) ||
                !Math::Equal(scale.z, 1.0f) ||
                !Math::Equal(angle, 0.0f))
            {
                return true;
            }
        }
        return false;
    }

    void NodeAnimationTrack::optimise()
    {
        // Eliminate duplicate keyframes from 2nd to penultimate keyframe
		// NB only eliminate middle keys from sequences of 5+ identical keyframes
		// since we need to preserve the boundary keys in place, and we need
		// 2 at each end to preserve tangents for spline interpolation

        // TODO: But now we don't use spline
        // So we duplicate middle keys from sequences of 3+ identical keyframes

        std::list<uint16_t> framesToDestroy;
        Vector3 lastTranslate = Vector3::ZERO;
        Quaternion lastRotate = Quaternion::IDENTITY;
        Vector3 lastScale = Vector3::ONE;
        uint16_t dupCount = 0;
        uint16_t k = 0;
        for (KeyFrameVectorIterator it=mKeyFrames.begin(); it!=mKeyFrames.end(); ++it)
        {
            TransformKeyFrame *keyFrame = dynamic_cast<TransformKeyFrame*>(*it);
            Vector3 newTranslate = keyFrame->getTranslate();
            Quaternion newRotate = keyFrame->getRotate();
            Vector3 newScale = keyFrame->getScale();
            if (it != mKeyFrames.begin() &&
                Math::Equal(newTranslate.x, lastTranslate.x) &&
                Math::Equal(newTranslate.y, lastTranslate.y) &&
                Math::Equal(newTranslate.z, lastTranslate.z) &&
                Math::Equal(newRotate.x, lastRotate.x) &&
                Math::Equal(newRotate.y, lastRotate.y) &&
                Math::Equal(newRotate.z, lastRotate.z) &&
                Math::Equal(newRotate.w, lastRotate.w) &&
                Math::Equal(newScale.x, lastScale.x) &&
                Math::Equal(newScale.y, lastScale.y) &&
                Math::Equal(newScale.z, lastScale.z))
            {
                ++dupCount;

                // 4 indicates this is the 5th duplicate keyframe
                // 2 indicates this is the 3rd duplicate keyframe
                if (dupCount == 2)
                {
                    // Mark the 'middle' keyframe for destruction
                    framesToDestroy.push_back(k - 1);
                    --dupCount;
                }
            }
            else
            {
                dupCount = 0;
                lastTranslate = newTranslate;
                lastRotate = newRotate;
                lastScale = newScale;
            }
        }

        // Now destroy the keyFrames we marked for death in reverse order
        for (std::list<uint16_t>::reverse_iterator i=framesToDestroy.rbegin(); i!=framesToDestroy.rend(); ++i)
        {
            destroyKeyFrame(*i);
        }
    }

    void NodeAnimationTrack::_keyFrameDataChanged() const
    {
        mSplineBuildNeeded = true;
    }

    NodeAnimationTrack::~NodeAnimationTrack()
    {
    }

    KeyFrame* NodeAnimationTrack::_createKeyFrameImpl(const real time)
    {
        return new TransformKeyFrame(this, time);
    }

    void NodeAnimationTrack::_buildInterpolationSplines() const
    {
    }
}
